#include "misc.h"

void transformPointsForward(vector<Point2f>&origin, const Matx33d &H,bool isBox,const cv::Rect &box)
{
    vector<Point2f> dst;
    const double *aa = H.val;
    dst = origin;
    for (int i = 0; i<dst.size(); i++) {
        auto &a = dst[i];
        auto &b = origin[i];
        double s = aa[6] * a.x + aa[7] * a.y + aa[8];
        b.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
        b.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
    }
}



void transformPointsForward(const vector<Feature*>&src,vector<Feature*>&dst,const Eigen::Matrix3d&H,const cv::Size&imgSize)
{
    size_t srcSize = src.size();
    size_t dstSize = dst.size();
    if (srcSize>dstSize) {
        dst.reserve(srcSize);
        for (int i=dstSize; i<srcSize; ++i) {
            dst.push_back(new Feature());
        }
    }
    else if(srcSize<dstSize)
    {
        for (int i=srcSize; i<dstSize; ++i) {
            delete dst[i];
        }
        dst.resize(srcSize);
    }
    for (int i = 0; i<src.size(); i++)
    {
        assert(dst[i]!=nullptr);
        auto &a = src[i]->_pos;
        auto &b = dst[i]->_pos;
        dst[i]->pointXYZ = src[i]->pointXYZ;
        double s = H(2,0) * a.x() + H(2,1) * a.y() + H(2,2);
        b.x() = (H(0,0) * a.x() + H(0,1) * a.y() + H(0,2)) / s;
        b.y() = (H(1,0) * a.x() + H(1,1) * a.y() + H(1,2)) / s;
        
        if (b.x()<0||b.x()>=imgSize.width||b.y()<0||b.y()>=imgSize.height)
            dst[i]->isBad() = true;
        else
            dst[i]->isBad() = false;
    }
    
}


void transformPointsInverse(vector<Point2f>&origin, const Matx33d &H,bool isBox,const cv::Rect &box)
{
    Matx33d invH;
    invert(H, invH);
    vector<Point2f> dst;
    dst = origin;
    const double *aa = invH.val;
    for (int i = 0; i<dst.size(); i++) {
        auto &a = dst[i];
        auto &b = origin[i];
        double s = aa[6] * a.x + aa[7] * a.y + aa[8];
        Point2f aaa;
        b.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
        b.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
    }
}


__inline double normL2(float x1, float y1, float x2, float y2)
{
    return sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
}


double computeAreaQuad(const vector<Point2f> &vertex)
{
    float a, b, c, d;
    a = normL2(vertex[0].x, vertex[0].y, vertex[1].x, vertex[1].y);
    b = normL2(vertex[1].x, vertex[1].y, vertex[2].x, vertex[2].y);
    c = normL2(vertex[2].x, vertex[2].y, vertex[3].x, vertex[3].y);
    d = normL2(vertex[3].x, vertex[3].y, vertex[0].x, vertex[0].y);
    double z = (a + b + c + d) / 2;
    return sqrt((z - a)*(z - b)*(z - c)*(z - d));
}

void setPyramid(const Mat &img, const int levelsNum, const float scaleFactor, vector<Mat> &imagePyramid)
{
    imagePyramid.resize(levelsNum);
    
    const int border = 5;
    const int firstLevel = 0;
    for (int level = 0; level < levelsNum; ++level)
    {
        float scale = 1 / pow(scaleFactor,level);
        cv::Size sz(cvRound(img.cols*scale), cvRound(img.rows*scale));
        cv::Size wholeSize(sz.width + border * 2, sz.height + border * 2);
        Mat temp(wholeSize, img.type()), masktemp;
        imagePyramid[level] = temp(Rect(border, border, sz.width, sz.height));
        
        // Compute the resized img
        if (level != firstLevel)
        {
            if (level < firstLevel)
            {
                resize(img, imagePyramid[level], sz, 0, 0, INTER_LINEAR);
            }
            else
            {
                resize(imagePyramid[level - 1], imagePyramid[level], sz, 0, 0, INTER_LINEAR);
            }
            
            copyMakeBorder(imagePyramid[level], temp, border, border, border, border,
                           BORDER_REFLECT_101 + BORDER_ISOLATED);
        }
        else
        {
            copyMakeBorder(img, temp, border, border, border, border,
                           BORDER_REFLECT_101);
        }
    }
}




Matx33d warpScale(const Mat&src, Mat&dst, const Matx33d H, float _scale, int level)
{
    double scale = 1 / pow(_scale, level);
    Matx33d s =Matx33d::eye();
    s(0, 0) = scale;
    s(1, 1) = scale;
    s = s*H;

    warpPerspective(src, dst, s, cv::Size(src.cols*scale,src.rows*scale));
    return s;
}